#pragma once
//this is the second version of bspline_opt_sep_impl.h, which uses
//more mathematics and less classes

#include "bspline_opt.h"
#include <mtx/dynamic.h>
#include <mtx/algorithm.h>
#include <glib/g2/algorithm.h> //decompose
//debug
//#include <mtx/utility.h>
//#include <iostream>

namespace outline {

// spline

///this class describes overdetermined set of equations for spline fitting:
///Ax=b (where x are all the variables of the spline)
///and an additional set of constraint (equations that must be exactly meeted):
///Cx=d
template <typename R>
class bspline_opt_base<R>::spline2 {
public:
	spline2(bool closed, uint nsize,
		std::vector<junc_type> const & junc, controls<R> const & ctrls, bool antiloop,
		point_vector const * attractors = 0, real_vector const * tractions = 0);
	///contructor calls update_constraints(...) once, but you should do it again
	///if the proportions of the tangents are changed somehow
	void update_constraints(std::vector<junc_type> const & junc, controls<R> const & ctrls, bool antiloop,
		point_vector const * attractors = 0, real_vector const * tractions = 0);
	uint freevars() const
		{ return vsize_ - csize_; }
public: //formulate and solve overdetermined equations
	void eq(uint piece, real t, point goal);
	void eq(uint piece, real t, point, point goal) { eq(piece, t, goal); }
	void eq_solve(controls<R> & ctrls,
		point_vector const * attractors = 0, real_vector const * tractions = 0);
	//eq_solve can return summed discrepancy of the solution:
	//chisq = x^T (A^T A) x - 2 x^T (A^T b) + |b|^2 = |b|^2 - x^T (A^T b),
	//but it will be a bit slower;
private:
	void eq_reset();
	void add_attraction(uint var, point const & attractor, real traction);
private:
	typedef mtx::dynamic::vector vector;
	typedef mtx::dynamic::matrix matrix;
private:
	bool closed_;
	uint nsize_; ///< the number of nodes in the spline
	uint vsize_; ///< the number of variables: (3*nsize_-2) for open splines; (3*nsize_) for closed splines
	matrix m_; ///< m = A^T A (only upper-right part)
	vector x_, y_; ///< x = A^T b_x; y = A^T b_y
	//constrains
	uint csize_; ///< the number of constrains
	vector Dx_, Dy_; ///< D = C^T d
	vector D1x_, D1y_; ///< D1 = C^T (C C^T)^-1 d
	matrix E_; ///< E = C^T C
	matrix F_; ///< F = I - C^T (C C^T)^-1 C
};

template <typename R>
bspline_opt_base<R>::spline2::spline2(bool closed, uint nsize,
	std::vector<junc_type> const & junc, controls<R> const & ctrls, bool antiloop,
	point_vector const * attractors, real_vector const * tractions)
{
	closed_ = closed;
	nsize_ = nsize;
	assert (closed_ && nsize_ >= 1 || !closed_ && nsize_ >= 2);
	vsize_ = closed_ ? (3*nsize_) : (3*nsize_-2);
	m_.resize(vsize_, vsize_);
	x_.resize(vsize_);
	y_.resize(vsize_);

	update_constraints(junc, ctrls, antiloop, attractors, tractions);
}

template <typename R>
void bspline_opt_base<R>::spline2::update_constraints(
	std::vector<junc_type> const & junc, controls<R> const & ctrls, bool antiloop,
	point_vector const * attractors, real_vector const * tractions)
{
	assert (junc.size() >= nsize_);
	assert (!attractors || attractors->size() >= vsize_ && tractions->size() >= vsize_);

	struct constrains {
		typedef std::vector<real> dvec;
		typedef std::vector<dvec> dmat;
		uint vsize;
		dmat C;
		dvec d_x, d_y;
		//remember here all fully fixed nodes and tangents
		std::vector<bool> fixed;
	public:
		constrains(uint vs) : vsize(vs), fixed(vs, false)
			{ C.reserve(vs); }
		uint count()
			{ return d_x.size(); }
		void fix(uint var, point d) {
			assert (var < vsize);
			assert (!fixed[var]);
			fixed[var] = true;
			C.push_back(dvec());
			dvec & c = C.back();
			c.resize(vsize, 0);
			c[var] = 1;
			d_x.push_back(d.x);
			d_y.push_back(d.y);
		}
		void fix_weighted_sum2(uint var0, real w0, uint var1, real w1, point d = point(0,0)) {
			assert (var0 < vsize && var1 < vsize && var0 != var1);
			C.push_back(dvec());
			dvec & c = C.back();
			c.resize(vsize, 0);
			c[var0] = w0;
			c[var1] = w1;
			d_x.push_back(d.x);
			d_y.push_back(d.y);
		}
		void fix_weighted_sum2_delta(uint v, real w, uint n0, uint n1, real wn, point d = point(0,0)) {
			assert (v < vsize && n0 < vsize && n1 < vsize && v != n0 && n0 != n1 && n1 != v);
			C.push_back(dvec());
			dvec & c = C.back();
			c.resize(vsize, 0);
			c[v] = w;
			c[n0] = -wn;
			c[n1] =  wn;
			d_x.push_back(d.x);
			d_y.push_back(d.y);
		}
		void fix_weighted_sum3_delta(uint va, real wa, uint vb, real wb, uint n0, uint n1, real wn, point d = point(0,0)) {
			assert (va < vsize && vb < vsize && n0 < vsize && n1 < vsize
				 && va != n0 && n0 != n1 && n1 != va && vb != n0 && va != vb && n1 != vb);
			C.push_back(dvec());
			dvec & c = C.back();
			c.resize(vsize, 0);
			c[va] = wa;
			c[vb] = wb;
			c[n0] = -wn;
			c[n1] =  wn;
			d_x.push_back(d.x);
			d_y.push_back(d.y);
		}
	} cns(vsize_);

	for (uint i = 0; i < nsize_; ++i) 
	{
		uint l = (i == 0) ? 3*nsize_-1 : 3*i-1; //not valid for open splines and i == 0
		bool has_l = closed_ || i > 0;
		uint c = 3*i;
		uint r = 3*i+1; //not valid for open splines and i == nsize_-1
		bool has_r = closed_ || i+1 < nsize_;

		if (attractors && (*tractions)[c] >= 1)
			cns.fix(c, (*attractors)[c]);

		switch (junc[i]) {
		default:
			assert (false);
		case junc_flat:
			if (has_l)
				cns.fix(l, point(0,0));
			if (has_r)
				cns.fix(r, point(0,0));
			break;
		case junc_smooth: 
		case junc_smooth_affine: 
		case junc_symmetrical: 
		{
			if (!closed_ && tractions) { 
				if (i == 0 && (*tractions)[r] >= 1) {
					cns.fix(r, (*attractors)[r] - (*attractors)[c]);
					break;
				}
				if (i+1 == nsize_ && (*tractions)[l] >= 1) {
					cns.fix(l, (*attractors)[l] - (*attractors)[c]);
					break;
				}
			}
			assert (closed_ || i > 0 && i+1 < nsize_);
			if (tractions && (*tractions)[l] >= 1 && (*tractions)[r] >= 1) {
				cns.fix(l, (*attractors)[l] - (*attractors)[c]);
				cns.fix(r, (*attractors)[r] - (*attractors)[c]);
				break;
			}

			real jl, jr;
			if (junc[i] == junc_symmetrical) 
				{ jl = -1; jr = 1; }
			else
				ctrls.calc_tangents_relength(l, c, r, jl, jr, true);
			cns.fix_weighted_sum2(l, jr, r, -jl);
			break;
		}
		case junc_cusp:
			if (attractors && has_l && (*tractions)[l] >= 1)
				cns.fix(l, (*attractors)[l] - (*attractors)[c]);
			if (attractors && has_r && (*tractions)[r] >= 1)
				cns.fix(r, (*attractors)[r] - (*attractors)[c]);
			break;
		}
	}

	if (antiloop) {
		//transformation of each bezier-segment must be affine
		for (uint s = 0; closed_ && s < nsize_ || !closed_ && s+1 < nsize_; ++s) {
			uint n0 = 3*s;
			uint l = n0 + 1;
			uint r = n0 + 2;
			uint n1 = n0 + 3;
			if (s+1 == nsize_) {
				assert (closed_);
				assert (n1 == vsize_);
				n1 = 0;
			}
			point left_tan = ctrls[l] - ctrls[n0];
			real left_tan2 = square(left_tan);
			point right_tan = ctrls[r] - ctrls[n1];
			real right_tan2 = square(right_tan);
			point delta = ctrls[n1] - ctrls[n0];
			real delta2 = square(delta);

			if (delta2 < std::numeric_limits<real>::epsilon())
				continue;

			//check for parallelism between a tangent and delta
			bool left_par = left_tan2 * delta2 * std::numeric_limits<real>::epsilon() 
				>= sqr(outer_product(left_tan, delta));
			if (left_par) {
				if (left_tan2 < delta2 * std::numeric_limits<real>::epsilon()) {
					if (!cns.fixed[l]) 
						cns.fix(l, point(0,0));
				} else
					cns.fix_weighted_sum2_delta(l, 1, n0, n1, -scalar_product(left_tan, delta) / delta2);
			}
			bool right_par = right_tan2 * delta2 * std::numeric_limits<real>::epsilon() 
				>= sqr(outer_product(right_tan, delta));
			if (right_par) {
				if (right_tan2 < delta2 * std::numeric_limits<real>::epsilon()) {
					if (!cns.fixed[r]) 
						cns.fix(r, point(0,0));
				} else
					cns.fix_weighted_sum2_delta(r, 1, n0, n1, -scalar_product(right_tan, delta) / delta2);
			}
			if (left_par || right_par)
				continue;

			//check for parallelism between two tangents
			if (right_tan2 * left_tan2 * std::numeric_limits<real>::epsilon() 
				>= sqr(outer_product(right_tan, left_tan)))
			{
				real s = scalar_product(right_tan, left_tan);
				cns.fix_weighted_sum2(l, s / sqrt(left_tan2), r, s / sqrt(right_tan2));
				continue;
			}

			//most general case
			g2::basic_vector<real> vv = delta;
			g2::decompose<real>(left_tan, right_tan, vv);
			cns.fix_weighted_sum3_delta(l, vv.x, r, vv.y, n0, n1, -1);
		}
	}

	csize_ = cns.count();
	assert (vsize_ >= csize_);
	if (csize_ > 0) {
		// E = C^T C
		E_.resize(vsize_, vsize_); 
		//(only upper-right part)
		for (uint r = 0; r < vsize_; ++r) {
			for (uint c = r; c < vsize_; ++c) {
				double & s = E_(r, c);
				s = 0;
				for (uint i = 0; i < csize_; ++i)
					s += cns.C[i][r] * cns.C[i][c];
			}
		}
		//make E_ symmetric by copying upper-right part into lower-left part
		for (uint r = 0; r < vsize_; ++r)
			for (uint c = 0; c < r; ++c)
				E_(r, c) = E_(c, r);

		// G = C C^T
		matrix G;
		G.resize(csize_, csize_);
		for (uint r = 0; r < csize_; ++r) {
			for (uint c = 0; c < csize_; ++c) {
				double & s = G(r, c);
				s = 0;
				for (uint i = 0; i < vsize_; ++i)
					s += cns.C[r][i] * cns.C[c][i];
			}
		}
		// G := (C C^T)^-1
		inverse(G);

		// H = C^T (C C^T)^-1
		matrix H;
		H.resize(vsize_, csize_);
		for (uint r = 0; r < vsize_; ++r) {
			for (uint c = 0; c < csize_; ++c) {
				double & s = H(r, c);
				s = 0;
				for (uint i = 0; i < csize_; ++i)
					s += cns.C[i][r] * G(i, c);
			}
		}

		// F = I - C^T (C C^T)^-1 C
		F_.resize(vsize_, vsize_); 
		for (uint r = 0; r < vsize_; ++r) {
			for (uint c = 0; c < vsize_; ++c) {
				double & s = F_(r, c);
				s = (r == c) ? 1 : 0;
				for (uint i = 0; i < csize_; ++i)
					s -= H(r,i) * cns.C[i][c];
			}
		}

		// D = C^T d
		Dx_.resize(vsize_);
		Dy_.resize(vsize_);
		// D1 = C^T (C C^T)^-1 d = H d
		D1x_.resize(vsize_);
		D1y_.resize(vsize_);
		for (uint v = 0; v < vsize_; ++v) {
			Dx_(v) = Dy_(v) = 0;
			D1x_(v) = D1y_(v) = 0;
			for (uint c = 0; c < csize_; ++c) {
				Dx_(v) += cns.C[c][v] * cns.d_x[c];
				Dy_(v) += cns.C[c][v] * cns.d_y[c];
				D1x_(v) += H(v, c) * cns.d_x[c];
				D1y_(v) += H(v, c) * cns.d_y[c];
			}
		}
	}

	eq_reset();
}

template <typename R>
inline void bspline_opt_base<R>::spline2::eq_reset()
{
	m_ = 0;
	x_ = 0;
	y_ = 0;
}

template <typename R>
void bspline_opt_base<R>::spline2::eq(uint piece, real t, point goal)
{
	assert (piece >= 0);
	assert (closed_ && piece < nsize_ || !closed_ && piece+1 < nsize_);

	real u = 1-t;
	real wn0 = u*u*(1+2*t);
	real wt0 = 3*u*u*t;
	real wt1 = 3*u*t*t;
	real wn1 = t*t*(3-2*t);

	uint const n0 = piece * 3;
	uint const t0 = n0 + 1;
	uint const t1 = n0 + 2;

	if (nsize_ == 1) {
		assert (closed_);
		//last and the only piece in closed contour
		wn0 = wn0 + wn1;
	}

	x_(n0) += wn0 * goal.x;
	y_(n0) += wn0 * goal.y;
	x_(t0) += wt0 * goal.x;
	y_(t0) += wt0 * goal.y;
	x_(t1) += wt1 * goal.x;
	y_(t1) += wt1 * goal.y;

	//fill only upper-right part of m_
	m_(n0, n0) += wn0 * wn0;
	m_(n0, t0) += wn0 * wt0;
	m_(n0, t1) += wn0 * wt1;
	m_(t0, t0) += wt0 * wt0;
	m_(t0, t1) += wt0 * wt1;
	m_(t1, t1) += wt1 * wt1;

	if (nsize_ > 1) {
		if (piece+1 == nsize_) {
			//last piece in closed spline
			uint const n1 = 0;
			assert (n1 < n0);

			x_(n1) += wn1 * goal.x;
			y_(n1) += wn1 * goal.y;

			//fill only upper-right part of m_
			m_(n1, n1) += wn1 * wn1;
			m_(n1, n0) += wn1 * wn0;
			m_(n1, t0) += wn1 * wt0;
			m_(n1, t1) += wn1 * wt1;
		}
		else {
			uint const n1 = n0 + 3;

			x_(n1) += wn1 * goal.x;
			y_(n1) += wn1 * goal.y;

			//fill only upper-right part of m_
			m_(n0, n1) += wn0 * wn1;
			m_(t0, n1) += wt0 * wn1;
			m_(t1, n1) += wt1 * wn1;
			m_(n1, n1) += wn1 * wn1;
		}
	}
}

template <typename R>
void bspline_opt_base<R>::spline2::eq_solve(controls<R> & ctrls,
	point_vector const * attractors, real_vector const * tractions)
{
	assert (closed_ && ctrls.pieces() == nsize_ || !closed_ && ctrls.pieces()+1 == nsize_);

	assert (vsize_ > 0);

	//make m_ symmetric by copying upper-right part into lower-left part
	for (uint r = 0; r < vsize_; ++r)
		for (uint c = 0; c < r; ++c)
			m_(r, c) = m_(c, r);

	// we are going to solve equations
	// m_ * x1 = x_;
	// m_ * y1 = y_;

	//modify equations according to the tractions
	if (attractors) {
		for (uint i = 0; i < nsize_; ++i) 
		{
			uint l = (i == 0) ? 3*nsize_-1 : 3*i-1; //not valid for open splines and i == 0
			uint c = 3*i;
			uint r = 3*i+1; //not valid for open splines and i == nsize_-1

			point center(x_(c), y_(c));
			if (closed_ || i > 0)
				add_attraction(l, (*attractors)[l] - (*attractors)[c], (*tractions)[l]);
			add_attraction(c, (*attractors)[c], (*tractions)[c]);
			if (closed_ || i+1 < nsize_)
				add_attraction(r, (*attractors)[r] - (*attractors)[c], (*tractions)[r]);
		}
	}

	if (csize_ > 0) { 
		//modify equations according to the constrains

		// K = F^T * m_
		matrix K;
		K.resize(vsize_, vsize_);
		for (uint r = 0; r < vsize_; ++r) {
			for (uint c = 0; c < vsize_; ++c) {
				double & s = K(r, c);
				s = 0;
				for (uint i = 0; i < vsize_; ++i)
					s += F_(i,r) * m_(i,c);
			}
		}

		// m_ := E_ + K * F = E_ + F^T * m_ * F_
		for (uint r = 0; r < vsize_; ++r) {
			for (uint c = 0; c < vsize_; ++c) {
				double & s = m_(r, c);
				s = E_(r, c);
				for (uint i = 0; i < vsize_; ++i)
					s += K(r,i) * F_(i,c);
			}
		}

		// x = Dx_ + F_^T x_ - K * D1x_
		// y = Dy_ + F_^T y_ - K * D1y_
		vector xx, yy;
		xx.resize(vsize_);
		yy.resize(vsize_);
		for (uint i = 0; i < vsize_; ++i) {
			double & x = xx(i);
			x = Dx_(i);
			double & y = yy(i);
			y = Dy_(i);
			for (uint j = 0; j < vsize_; ++j) {
				x += F_(j,i) * x_(j) - K(i,j) * D1x_(j);
				y += F_(j,i) * y_(j) - K(i,j) * D1y_(j);
			}
		}
		x_ = xx;
		y_ = yy;
	}

	//solve equations
	//std::clog << "m:\n" << m_;
	//std::clog << "x:\n" << x_;
	//std::clog << "y:\n" << y_;
	uint_vector pivot(vsize_-1);
	LU(m_, pivot);
	sle(m_, x_, pivot);
	sle(m_, y_, pivot);
	//std::clog << "m-lup:\n" << m_;
	//std::clog << "x-sol:\n" << x_;
	//std::clog << "y-sol:\n" << y_;

	//copy result into ctrls
	for (uint i = 0; i < nsize_; ++i) 
	{
		uint l = (i == 0) ? 3*nsize_-1 : 3*i-1; //not valid for open splines and i == 0
		uint c = 3*i;
		uint r = 3*i+1; //not valid for open splines and i == nsize_-1

		point center(x_(c), y_(c));
		if (closed_ || i > 0)
			ctrls[l] = center + point(x_(l), y_(l));
		ctrls[c] = center;
		if (closed_ || i+1 < nsize_)
			ctrls[r] = center + point(x_(r), y_(r));
	}
	if (closed_)
		ctrls[3*nsize_] = ctrls[0];

	eq_reset();
}

template <typename R>
inline void bspline_opt_base<R>::spline2::add_attraction(uint var, point const & attractor, real traction)
{
	if (traction <= 0 || traction >= 1)
		return; //either variable is free or it is fully fixed
	real tt = square(traction / (1 - traction));
	double & coef = m_(var, var);
	x_(var) += coef * attractor.x * tt;
	y_(var) += coef * attractor.y * tt;
	coef *= (1 + tt);
}

} //namespace outline
